
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_math.c
  * @author     baiyang
  * @date       2021-7-5
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_math.h"
#include <stdlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/** 
  * @brief       反sin值
  * @param[in]   val  
  * @param[out]  
  * @retval      
  * @note        输出范围（-pi/2，pi/2）
  */
float math_asinf(const float val)
{
    if (isnan(val))
    {
        return 0.0f;
    }
    if (val > 1.0f)
    {
        return (float)M_PI_2;
    }
    if (val <= -1.0f)
    {
        return (float)-M_PI_2;
    }
    return asinf(val);
}

/** 
  * @brief       开方运算
  * @param[in]   val  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_sqrtf(float val)
{
    float ret = sqrtf(val);
    if (isnan(ret))
    {
        return 0.0f;
    }
    return ret;
}

/** 
  * @brief       开方取倒数
  * @param[in]   number  
  * @param[out]  
  * @retval      
  * @note        用于向量单位化
  */
float math_rsqrtf(float number)
{
    long i;
    float x2, y;
    const float threehalfs = 1.5F;

    x2 = number * 0.5F;
    y  = number;
    i  = * ( long * ) &y;                       
    i  = 0x5f3759df - ( i >> 1 );               
    y  = * ( float * ) &i;
    y  = y * ( threehalfs - ( x2 * y * y ) );   
    y  = y * ( threehalfs - ( x2 * y * y ) );   

    return y;
}

/** 
  * @brief       限幅
  * @param[in]   amt  
  * @param[in]   low  
  * @param[in]   high  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_constrain_float(float amt, float low, float high)
{
    if (isnan(amt))
    {
        return (low + high) * 0.5f;
    }

    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}

/** 
  * @brief       限幅
  * @param[in]   amt  
  * @param[in]   low  
  * @param[in]   high  
  * @param[out]  
  * @retval      
  * @note        
  */
double math_constrain_double(double amt, double low, double high)
{
    if (isnan(amt))
    {
        return (low + high) * 0.5f;
    }

    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}

/**
  * @brief       
  * @param[in]   mean  
  * @param[in]   stddev  
  * @param[out]  
  * @retval      
  * @note        
  */
double math_rand_normal(double mean, double stddev)
{
    static double n2 = 0.0;
    static int n2_cached = 0;

    if (!n2_cached)
    {
        double x, y, r;
        do
        {
            x = 2.0 * rand() / RAND_MAX - 1;
            y = 2.0 * rand() / RAND_MAX - 1;
            r = x * x + y * y;
        } while (math_dbl_zero(r) || (r > 1.0));
        const double d = sqrt(-2.0 * log(r) / r);
        const double n1 = x * d;
        n2 = y * d;
        const double result = n1 * stddev + mean;
        n2_cached = 1;
        return result;
    }
    else
    {
        n2_cached = 0;
        return n2 * stddev + mean;
    }
}

/** 
  * @brief       线性插值
  * @param[in]   low_output  
  * @param[in]   high_output  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_linear_inter(float low_output, float high_output,
                         float var_value,
                         float var_low, float var_high)
{
    if (var_value <= var_low) 
    {
        return low_output;
    }
    if (var_value >= var_high) 
    {
        return high_output;
    }

    float p = (var_value - var_low) / (var_high - var_low);

    return low_output + p * (high_output - low_output);
}


/** 
  * @brief       弧度值限幅-PI PI
  * @param[in]   radian  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_wrap_PI(const float radian)
{
    float res = math_wrap_2PI(radian);
    if (res > M_PI)
    {
        res -= M_2PI;
    }

    return res;
}

/** 
  * @brief       弧度值限幅 0 2PI
  * @param[in]   radian  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_wrap_2PI(const float radian)
{
    //取余
    float res = fmodf(radian, M_2PI);
    if (res < 0)
    {
        res += M_2PI;
    }

    return res;
}

/** 
  * @brief       度值限幅0 360
  * @param[in]   angle  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_wrap_360(const float angle)
{
    float res = fmodf(angle, 360.0f);
    if (res < 0)
    {
        res += 360.0f;
    }

    return res;
}

/** 
  * @brief       100度值限幅0 360
  * @param[in]   angle  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_wrap_360_cd(float angle)
{
    float res = fmodf(angle, 36000.0f);
    if (res < 0) 
    {
        res += 36000.0f;
    }

    return res;
}

/** 
  * @brief       度值限幅-180 180
  * @param[in]   angle  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_wrap_180(float angle)
{
    float res = math_wrap_360(angle);
    if (res > 180.0f) 
    {
        res -= 360.0f;
    }

    return res;
}

/** 
  * @brief       100度限幅 +-180000
  * @param[in]   angle  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_wrap_180_cd(float angle)
{
    float res = math_wrap_360_cd(angle);
    if (res > 18000.0f) 
    {
        res -= 36000.0f;
    }

    return res;
}

/** 
  * @brief       计算低通滤波器的alpha值
  * @param[in]   dt  
  * @param[in]   cutoff_freq  
  * @param[out]  
  * @retval      
  * @note        
  */
float math_calc_lpf_alpha_dt(float dt, float cutoff_freq)
{
    if (dt <= 0.0f || cutoff_freq <= 0.0f) {
        return 1.0;
    }
    float rc = 1.0f/(M_2PI*cutoff_freq);
    return math_constrain_float(dt/(dt+rc), 0.0f, 1.0f);
}

/*
  simple 16 bit random number generator
 */
uint16_t math_get_random16(void)
{
    static uint32_t m_z = 1234;
    static uint32_t m_w = 76542;
    m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16);
    m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16);
    return ((m_z << 16) + m_w) & 0xFFFF;
}

/*------------------------------------test------------------------------------*/


